#include "rviz_custom_plugins/viewmodels/interactive_marker_model.h"
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <algorithm>
#include <rviz_common/logging.hpp>
#include "rviz_custom_plugins/utils/interactive_marker_util.h"


using rviz_custom_plugins::utils::addMetadata;


namespace rviz_custom_plugins
{

namespace viewmodels
{


InteractiveMarkerModel::InteractiveMarkerModel(QObject *parent): QStandardItemModel(parent) {
    setColumnCount(4);
    setHorizontalHeaderLabels({ "x", "y", "z", "yaw" });
}


Qt::ItemFlags InteractiveMarkerModel::flags(const QModelIndex& index) const {
    // 第一列不可编辑
    // if (index.column() == 0) {
    //     return QStandardItemModel::flags(index) & ~Qt::ItemIsEditable;
    // }
    return QStandardItemModel::flags(index) | Qt::ItemIsSelectable | Qt::ItemIsEditable | Qt::ItemIsEnabled;
}


bool InteractiveMarkerModel::setData(const QModelIndex& index, const QVariant& value, int role)
{
    if (!index.isValid())
        return false;

    bool success = false;
    if (role == Qt::EditRole) {
        // 获取对应行的标记
        auto it = markers.begin();
        std::advance(it, index.row());

        // 更新对应字段
        if(it != markers.end()) {
            switch (index.column()) {
                case 0: it->pose.position.x = value.toDouble(); break;
                case 1: it->pose.position.y = value.toDouble(); break;
                case 2: it->pose.position.z = value.toDouble(); break;
                case 3: {
                    tf2::Quaternion newQuat;
                    // from roll,pitch,yaw to quat
                    newQuat.setRPY(0, 0, value.toDouble());
                    it->pose.orientation.w = newQuat.w();
                    it->pose.orientation.x = newQuat.x();
                    it->pose.orientation.y = newQuat.y();
                    it->pose.orientation.z = newQuat.z();
                    break;
                }
                default:
                    return false;
            }
        }
        // 调用基类方法更新显示
        success = QStandardItemModel::setData(index, value, role);
        if (success) {
            emit dataChanged(index, index, {role});
            emit markerEditUpdated(*it); // 自定义信号，通知外部数据变更
        }
    } else {
        success = QStandardItemModel::setData(index, value, role);
    }
    return success;
}


bool InteractiveMarkerModel::insertMarker(int row, InteractiveMarker& intMarker) {
    if (row > rowCount()) {
        return false;
    }

    auto it = markers.begin();
    std::advance(it, row);
    markers.insert(it, intMarker);
    
    tf2::Quaternion quat(
        intMarker.pose.orientation.x,
        intMarker.pose.orientation.y,
        intMarker.pose.orientation.z,
        intMarker.pose.orientation.w
    );
    double roll, pitch, yaw;
    tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw); // 提取yaw角

    QList<QStandardItem*> pointRow;
    // pointRow << new QStandardItem(QString::fromStdString(intMarker.name));
    pointRow << new QStandardItem(QString::number(intMarker.pose.position.x,'f', 2));
    pointRow << new QStandardItem(QString::number(intMarker.pose.position.y,'f', 2));
    pointRow << new QStandardItem(QString::number(intMarker.pose.position.z,'f', 2));
    pointRow << new QStandardItem(QString::number(yaw,'f', 2));
    insertRow(row, pointRow);

    handlePointsReorder(row);

    emit markerOrderUpdated(row, markers);
    return true;
}


void InteractiveMarkerModel::handleMarkerFeedback(InteractiveMarkerServer::FeedbackConstSharedPtr feedback) {
    auto it = std::find_if(markers.begin(), markers.end(), [feedback](const InteractiveMarker& im) { return im.name==feedback->marker_name; });
    if (it != markers.end()) {
        it->pose = feedback->pose;
    }
    QString markerName = QString::fromStdString(feedback->marker_name);
    int row = getMarkerRow(markerName);
    if(row >= 0) {
        tf2::Quaternion quat(
            feedback->pose.orientation.x,
            feedback->pose.orientation.y,
            feedback->pose.orientation.z,
            feedback->pose.orientation.w
        );
        double roll, pitch, yaw;
        tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw); // 提取yaw角
        setData(index(row, 0), QString::number(feedback->pose.position.x,'f', 2), Qt::DisplayRole);
        setData(index(row, 1), QString::number(feedback->pose.position.y,'f', 2), Qt::DisplayRole);
        setData(index(row, 2), QString::number(feedback->pose.position.z,'f', 2), Qt::DisplayRole);
        setData(index(row, 3), QString::number(yaw, 'f', 2),  Qt::DisplayRole);
    }
}



void InteractiveMarkerModel::removeMarker(int row) {
    auto it = markers.begin();
    std::advance(it, row);
    markers.erase(it);

    removeRow(row);
    handlePointsReorder(row);

    emit markerOrderUpdated(row, markers);
}


void InteractiveMarkerModel::handlePointsReorder(int row)
{
    auto markerIt = markers.begin();
    std::advance(markerIt, row); // 移动到指定索引
    int tmpRow = row;
    for ( ;markerIt != markers.end(); ++markerIt, ++tmpRow) {
        // 重新设置marker的row
        // QJsonObject metadata;
        // metadata[ROW] = QString::number(tmpRow);
        // setMetadata(*markerIt, metadata);
        addMetadata(*markerIt, ROW, QString::number(tmpRow));
    }
}



InteractiveMarker& InteractiveMarkerModel::getRowMarker(int row) {
    auto it = markers.begin();
    std::advance(it, row);
    return *it;
}


int InteractiveMarkerModel::getMarkerRow(QString& markerName) {
    auto it = std::find_if(markers.begin(), markers.end(), [markerName](const InteractiveMarker& im) { return im.name==markerName.toStdString(); });
    if (it != markers.end()) {
        return std::distance(markers.begin(), it);
    }
    return -1;
}


std::list<InteractiveMarker>& InteractiveMarkerModel::getMarkers()
{
    return markers;
}


}

}
